#include "global.h"
#include <netinet/in.h>
#include<sys/ioctl.h>
#include <thread>
#include "CSerialPort.h"

using namespace std;
typedef pcl::PointXYZ PointT;

//-------------------CSerialPort & thread---------------------------
static unsigned char re_msg =  0x04 ;
char mode = 0x04;
void ReadSerial();

int fd;
fd_set fs_read;
struct termios option;
CSerialPort myserialport(fd, option, fs_read);
char send_data[8];
//-------------------function---------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr points_to_pcl(const rs2::points& points);


//-------------------realsense & global variable---------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_fil(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<PointT>::Ptr cloud_viewer(new pcl::PointCloud<PointT>);

pcl::visualization::CloudViewer viewer("viewer");
char c='1';
//----------------------------------------------------

int main()
{

	std::thread *read_thresd = new std::thread(ReadSerial);
	while (true)
	{
		rs2::pointcloud pc;
		rs2::points points;
		rs2::pipeline pipe;
		pipe.start();
		int n=0;
		std::string title;
		while(true)
		{			
			if (mode == 0x04)//getAmmo
			{
				auto frames = pipe.wait_for_frames();
				auto depth = frames.get_depth_frame();
				points	 = pc.calculate(depth);

				cloud_filtered = points_to_pcl(points);
				pcl::PointCloud<pcl::PointXYZ> cloud1;
				cloud1.operator+=(*cloud_filtered);
				std::string title = std::to_string(n) + ".pcd";
				pcl::io::savePCDFileASCII (title, cloud1); //将点云保存到PCD文件中
				viewer.showCloud(cloud_filtered);
				n++;
			}
			usleep(400000);
		}
	}
}

void ReadSerial()
{
	while (1)
	{
		if (re_msg == 0x01)
		{
			mode = 0x01;
		}
		else if (re_msg == 0x02)
		{
			mode = 0x02;
		}
		else if(re_msg == 0x03)
		{
			mode = 0x03;
		}
		usleep(1000);
	}
}

pcl::PointCloud<pcl::PointXYZ>::Ptr points_to_pcl(const rs2::points& points)
{
    auto sp = points.get_profile().as<rs2::video_stream_profile>();
    cloud->width = sp.width();
    cloud->height = sp.height();
    cloud->is_dense = true;
    cloud->points.resize(points.size());
	cloud_fil->points.resize(0);
   
    auto ptr = points.get_vertices();
	
    for (auto& p : cloud->points)
    {
		
		p.x = ptr->x;
        p.y = ptr->y;
        p.z = ptr->z;
	
		cloud_fil->points.push_back(p);
		 ptr++;
    }
	
    return cloud_fil;
}

// void enable_raw_mode()
// {
//     termios term;
//     tcgetattr(0, &term);
//     term.c_lflag &= ~(ICANON | ECHO); // Disable echo as well
//     tcsetattr(0, TCSANOW, &term);
// }

// void disable_raw_mode()
// {
//     termios term;
//     tcgetattr(0, &term);
//     term.c_lflag |= ICANON | ECHO;
//     tcsetattr(0, TCSANOW, &term);
// }

// void keyinput()
// {
//    enable_raw_mode();
//     while(1)
//     {
		
//         usleep(10);
// 		c=getchar();
// 		if(c=='c')
// 		{
// 			if(mode==0x02)mode=re_msg=0x03;
// 			else if(mode==0x03)mode=re_msg=0x02;
// 		}
// 		else if(c=='q') y_min_getammo+=0.005;
// 		else if(c=='a') y_min_getammo-=0.005;
// 		else if(c=='w') y_max_getammo+=0.005;
// 		else if(c=='s') y_max_getammo-=0.005;
// 		else if(c=='o') y_min_plane+=0.005;
// 		else if(c=='k') y_min_plane-=0.005;
// 		else if(c=='p') y_max_plane+=0.005;
// 		else if(c=='l') y_max_plane-=0.005;
//     }
// 	disable_raw_mode();
//     tcflush(0, TCIFLUSH); // Clear stdin to prevent characters appearing on prompt
// }